package com.favoritemedium.util.geolocation;


/**
 * Implementation that uses an allegedly accurate formula devised 
 * by Thaddeus Vincenty in 1975
 * 
 * @author F. Azzam
 * 
 */
public class DistCalcVincenty extends BaseDistCalc implements IDistCalc {

	public DistCalcVincenty() {

	}

	public double calculateDistance(double lat1, double lon1, double lat2,
			double lon2, String unit) {
		return calculateDistanceV(lat1, lon1, lat2, lon2, unit).doubleValue();
	}

	private Double calculateDistanceV(double lat1, double lon1, double lat2,
			double lon2, String unit) {

		/*
		 * Calculate geodesic distance (in m) between two points specified by
		 * latitude/longitude (in numeric degrees) using Vincenty inverse
		 * formula for ellipsoids
		 */

		double a = 6378137, b = 6356752.3142, f = 1 / 298.257223563; // WGS-84
																		// ellipsoid
		double L = deg2rad(lon2 - lon1);
		double U1 = Math.atan((1 - f) * Math.tan(deg2rad(lat1)));
		double U2 = Math.atan((1 - f) * Math.tan(deg2rad(lat2)));
		double sinU1 = Math.sin(U1), cosU1 = Math.cos(U1);
		double sinU2 = Math.sin(U2), cosU2 = Math.cos(U2);

		double cosSqAlpha = 0;
		double sinSigma = 0;
		double cos2SigmaM = 0;
		double cosSigma = 0;
		double sigma = 0;

		double lambda = L, lambdaP = 2 * Math.PI;
		double iterLimit = 20;
		while (Math.abs(lambda - lambdaP) > 1e-12 && --iterLimit > 0) {
			double sinLambda = Math.sin(lambda), cosLambda = Math.cos(lambda);
			sinSigma = Math.sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda)
					+ (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda)
					* (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
			if (sinSigma == 0)
				return new Double(0); // co-incident points
			cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
			sigma = Math.atan2(sinSigma, cosSigma);
			double sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
			cosSqAlpha = 1 - sinAlpha * sinAlpha;
			cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
			Double cos2SigmaMObject = new Double(cos2SigmaM);
			if (cos2SigmaMObject.isNaN())
				cos2SigmaM = 0; // equatorial line: cosSqAlpha=0 (�6)
			double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
			lambdaP = lambda;
			lambda = L
					+ (1 - C)
					* f
					* sinAlpha
					* (sigma + C
							* sinSigma
							* (cos2SigmaM + C * cosSigma
									* (-1 + 2 * cos2SigmaM * cos2SigmaM)));
		}
		if (iterLimit == 0)
			return null; // formula failed to converge

		double uSq = cosSqAlpha * (a * a - b * b) / (b * b);
		double A = 1 + uSq / 16384
				* (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
		double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
		double deltaSigma = B
				* sinSigma
				* (cos2SigmaM + B
						/ 4
						* (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) - B
								/ 6 * cos2SigmaM
								* (-3 + 4 * sinSigma * sinSigma)
								* (-3 + 4 * cos2SigmaM * cos2SigmaM)));
		double s = b * A * (sigma - deltaSigma);

		s = roundDouble(s, 2); // round to 1mm precision
		return new Double(s);

	}

	public static void main(String[] args) {
		IDistCalc calculator = new DistCalcVincenty();
		System.out.println(calculator.calculateDistance(5.30000, 100.28333,
				3.13333, 101.5500, "M"));
	}
}
